package edu.mapi.ir.controllers;

import java.awt.geom.Point2D;
import java.util.Vector;

import edu.mapi.ir.planning.AStarSearch;
import edu.mapi.ir.state.EstimatedState;
import edu.mapi.ir.viewer.Maze;

/**
 * This controller calculates the optimal path between to points and drives the robot through this path.<br>
 * It needs to know the map to correctly calculate such path.
 * @author ZP
 */
public class AStarController extends AbstractController {
	
	protected Vector<Point2D> bestPath;
	protected int index = 0;
	protected AbstractController currentController = null;
	
	/**
	 * Given a maze and two points, calculates and drives the mouse through the best path
	 * @param maze The maze
	 * @param start The starting point (current position)
	 * @param target The destination (beacon)
	 */
	public AStarController(Maze maze, Point2D start, Point2D target, double initialRot) {
		AStarSearch search = new AStarSearch(maze);	
		bestPath = search.getBestPath(start, target, initialRot);
		currentController = new GoToController(start.getX(), start.getY(), bestPath.get(0).getX(), bestPath.get(0).getY());
	}
	
	public AStarController(Vector<Point2D> path) {
		bestPath = path;
		if (path == null || path.size() < 2)
			return;
		index = 1;
		currentController = new GoToController(bestPath.get(0).getX(), bestPath.get(0).getY(), bestPath.get(1).getX(), bestPath.get(1).getY());
	}
	
	/**
	 * If no start and ending points are given, it calculates the best path between position 0 of the grid and the first beacon
	 * @param maze
	 */
	public AStarController(Maze maze) {
		this(maze, maze.getGrid().firstElement().position, maze.getBeacon().getPosition(), Math.toRadians(maze.getGrid().firstElement().direction));			
	}
	
	/**
	 * Generates GoTo controllers to navigate through all points in the path
	 */
	public Actuation decide(Actuation lastActuation, EstimatedState state) {
		if (currentController == null)
			return new StopController().decide(lastActuation, state);
		
		if (currentController.isComplete()) {
			index++;
			if (index >= bestPath.size()) {
				setComplete(true);
				return new StopController().decide(lastActuation, state);
			}
			currentController = new GoToController(state.getX(), state.getY(), bestPath.get(index).getX(), bestPath.get(index).getY());
		}
		
		return currentController.decide(lastActuation, state);		
	}
}
